#include <string.h>
#include <math.h>
#include "ZR_API.h"
#include "ZR_API_internal.h"
#include "math_matrix.h"
#include "spheres_constants.h"
#include "spheres_types.h"
#include "pads.h"
#include "comm.h"
#include "gsp.h"
#include "gsp_task.h"
#include "control.h"
#include "ctrl_attitude.h"
#include "find_state_error.h"
#include "ctrl_mix.h"
#include "ZRGameInternal.h"

unsigned char simRan = 0;
static unsigned char userSetVelTarget;
static unsigned char userSetForces;
static unsigned char userSetTorques;
static unsigned char userSetPosTarget;
static unsigned char userSetAttTarget;

#ifdef ZRFLATFLOOR
static unsigned int lastSimUpdate;
#endif

static float userForces[3];
static float userTorques[3];
static float userVelTarget[3];
static float userPosTarget[3];
static float userAttTarget[3];


const float KPvelocityP = 2.0f;

void ZRAPIInit(){
#ifdef ZRFLATFLOOR	
	extern const unsigned int PADS_INERTIAL_TRIG;
	lastSimUpdate = 0;
	sixDOFSimAsBackgroundTelem();
	taskTriggerMaskSet(PADS_INERTIAL_TRIG);
#endif
	
	userSetVelTarget = 0;
	userSetForces = 0;
	userSetTorques = 0;
	userSetPosTarget = 0;
	userSetAttTarget = 0;
	
}

/** GETTERS AND SETTERS **/

void ZRSetForces(float *forces)
{
	memcpy(userForces,forces,sizeof(float)*3);
	userSetForces = 1;
}
unsigned char ZRGetForces(float *forces)
{
	unsigned char ret;
	ret = userSetForces;
	if (ret) {		
		userSetForces = 0;	//Clear user forces flag
		memcpy(forces,userForces,sizeof(float)*3);
	}
	return ret;
}
void ZRSetTorques(float *torques)
{
	memcpy(userTorques,torques,sizeof(float)*3);
	userSetTorques = 1;
}
unsigned char ZRGetTorques(float *torques)
{
	unsigned char ret;
	ret = userSetTorques;
	if (ret) {		
		userSetTorques = 0;	//Clear user torques flag
		memcpy(torques,userTorques,sizeof(float)*3);
	}
	return ret;
}

void ZRSetVelocityTarget(float *velTarget)
{
	memcpy(userVelTarget,velTarget,sizeof(float)*3);
	userSetVelTarget = 1;
}
unsigned char ZRGetVelocityTarget(float *velTarget)
{
	unsigned char ret;
	ret = userSetVelTarget;
	if (ret) {
		userSetVelTarget = 0;	//Clear user velocity flag
		memcpy(velTarget,userVelTarget,sizeof(float)*3);
	}
	return ret;
}

void ZRSetPositionTarget(float posTarget[3])
{
	memcpy(userPosTarget,posTarget,sizeof(float)*3);
	userSetPosTarget = 1;
}
unsigned char ZRGetPositionTarget(float *posTarget)
{
	unsigned char ret;
	ret = userSetPosTarget;
	if (ret) {		
		userSetPosTarget = 0;	//Clear user Position flag
		memcpy(posTarget,userPosTarget,sizeof(float)*3);
	}
	return ret;
}

void ZRSetAttitudeTarget(float attTarget[3])
{
	if (mathVecMagnitude(attTarget,3) > 0){
		memcpy(userAttTarget,attTarget,sizeof(float)*3);
		userSetAttTarget = 1;
	}
}
unsigned char ZRGetAttitudeTarget(float *attTarget)
{
	unsigned char ret;
	ret = userSetAttTarget;
	if (ret) {		
		userSetAttTarget = 0;	//Clear user Attitude flag
		memcpy(attTarget,userAttTarget,sizeof(float)*3);
	}
	return ret;
}

void ZRAttVec2Quat(float refVec[3], float attVec[3], float baseQuat[4], float quat[4])
{
	float sinTh;
	float cosTh;
	float rotAngle;
	float rotVec[3];
	int i;
	float tmpQuat[4];
	
	//normalize user attitude
	mathVecNormalize(attVec,3);
	//Find cross product between -x face and desired attitude
	mathVecCross(rotVec,refVec,attVec);
	//get cos of angle between vectors using the inner product
	cosTh = mathVecInner(refVec,attVec,3);
	//Get sin of angle between vectors using the magnitude of the cross product
	sinTh = mathVecMagnitude(rotVec,3);
	
	if (sinTh == 0.0)
	{
		//TODO: make this general again
		rotVec[0] = 0; rotVec[1] = 0; rotVec[2] = 1;
	}
	
	rotAngle = atan2f(sinTh,cosTh);
	//Normalize rotation vector
	mathVecNormalize(rotVec,3);
	//Calculate quaternion
	for (i=0; i<3; i++){
		tmpQuat[i] = rotVec[i]*sinf(rotAngle/2.0f);
	}
	tmpQuat[3] = cosf(rotAngle/2.0f);   
	
	quatMult(quat,tmpQuat,baseQuat);
    
} 

void ZRQuat2AttVec(float refVec[3], float quat[4], float attVec[3])
{
	float rotMatrix[3][3];
	quat2matrixOut(rotMatrix, quat);	
	mathMatVecMult(attVec, (float**)rotMatrix, refVec, 3, 3);
}

void ctrlVelocityPgains(float kpx, float kpy, float kpz, state_vector ctrlStateError, float ctrlControl[6])
{
	ctrlControl[FORCE_X] += kpx*ctrlStateError[VEL_X];
	ctrlControl[FORCE_Y] += kpy*ctrlStateError[VEL_Y];
	ctrlControl[FORCE_Z] += kpz*ctrlStateError[VEL_Z];
}

float dist3d(float *vecA, float *vecB)
{
	float distSquared = 0;
	unsigned char i;
	for (i=0; i<3; i++){
		distSquared += (vecA[i]-vecB[i])*(vecA[i]-vecB[i]);
	}
	return sqrtf(distSquared);
}

/** Grab ID from runtime command
 * Returns 255 if command is not valid
 */
unsigned char getZRUserIndexFromCommand(char cmd){
	unsigned char id = 0;
	if (cmd >= 0x31 && cmd <= 0x39){
		id = (cmd&0x0F)-1;
	} else {
		switch(cmd){
			case 'q':
			case 'Q':
				id = 9;
				break;
			case 'w':
			case 'W':
				id = 10;
				break;
			case 'e':
			case 'E':
				id = 11;
				break;
			case 'r':
			case 'R':
				id = 12;
				break;
			case 't':
			case 'T':
				id = 13;
				break;
			case 'y':
			case 'Y':
				id = 14;
				break;
			default:
				id = 255;
		}
	}
	return id;
}

void remapStates(state_vector ctrlState){
	float tmp;
	float tmpQuat[4];
	float yRotQuat[4] = {0.0f,0.0f,0.707f,0.707f};
	
	//Remap by rotating coordinate system -90 degrees about Z
	tmp = ctrlState[POS_X];
	ctrlState[POS_X] = -ctrlState[POS_Y];
	ctrlState[POS_Y] = tmp;
	tmp = ctrlState[VEL_X];
	ctrlState[VEL_X] = -ctrlState[VEL_Y];
	ctrlState[VEL_Y] = tmp;
	//Rotate about Z axis by 90 degrees
	memcpy(tmpQuat,&ctrlState[QUAT_1],sizeof(float)*4);
	quatMult(&ctrlState[QUAT_1],yRotQuat,tmpQuat);
	
	//On the flat floor, space the satellites out by 1.5m so they don't collide
#ifdef ZRFLATFLOOR
#if (SPHERE_ID == SPHERE1)
	ctrlState[POS_Y] = ctrlState[POS_Y] - 0.75f;
#else
	ctrlState[POS_Y] = ctrlState[POS_Y] + 0.75f;
#endif
#endif
}

/**
 * Retrieves the user state and partner satellite state.  If flat floor testing is enabled,
 * retrieves the simulatd 3D state.
 */
void padsStateGetZR(state_vector *ctrlState, state_vector *ctrlStatePartner){
	unsigned int tstamp;
	
	//Get normal state
	padsStateGet(*ctrlState);
	
	//Get partner state
#if(SPHERE_ID == SPHERE1)
	commBackgroundStateGet(SPHERE2, &tstamp, ctrlStatePartner);
#else
	commBackgroundStateGet(SPHERE1, &tstamp, ctrlStatePartner);
#endif
	
	//=========  RE-MAP AXES OF GAME TO MATCH SIMULATION =========//
#if(GAME_AXIS==POS_Y) //Re-map X axis to Y axis in ISS
#ifndef ZRSIMULATION //Don't do re-mapping in simulation to preserve SPHERES x,y,z views
	remapStates(*ctrlState);
	
	//Partner state will already be in correct coordinate system if simulating on flat floor
#ifndef ZRFLATFLOOR
	remapStates(*ctrlStatePartner);
#endif
	
#endif
#endif
	
#ifdef ZRFLATFLOOR
	//For flat floor, get state from the simulation
	sixDOFSimStateGet(*ctrlState);
#endif
	
}

#ifdef ZRFLATFLOOR
void sixDOFSimRun(unsigned int gsp_task_trigger, unsigned int extra_data){
	state_vector estState;
	state_vector simState;
	unsigned int steps, stepSize;
	unsigned int i;
	simRan = 1;
	if (gsp_task_trigger == PADS_INERTIAL_TRIG){	
		//Guard against erroneous updates
		if (ctrlTestTimeGet() < lastSimUpdate){
			return;
		}	
		stepSize = ctrlTestTimeGet()-lastSimUpdate;
		steps = stepSize / 50;
		
		//Guard against huge time steps
		if (stepSize > 10000){
			return;
		}
		
		//Break into 50ms steps
		for (i = 0; i<steps; i++){
			sixDOFSimStep(50);
		}
		//Simulate any remaining time
		if (steps*50 < stepSize){
			sixDOFSimStep(stepSize-steps*50);
		}
		lastSimUpdate += stepSize;
	}
	
	//Get the estimated state
	padsStateGet(estState);
#if(GAME_AXIS==POS_Y) //Re-map +X axis to -Y axis
	remapStates(estState);
#endif
	
	//Correct the X, Y position and velocity with the true state
	sixDOFSimStateGet(simState);
	simState[POS_X] = estState[POS_X];
	simState[POS_Y] = estState[POS_Y];
	simState[VEL_X] = estState[VEL_X];
	simState[VEL_Y] = estState[VEL_Y];
	sixDOFSimStateSet(simState);
	
#ifdef SPH_MATLAB_SIM
	//Refresh simulation with background telem
	commBackgroundPointerSet(&simState);
#endif
}

void ZRSimForceTorque(prop_time *firing_times, float *ctrlControl){
	state_vector flatFloorState;
	
#if SPHERE_ID == SPHERE1	
	state_vector attTarget = {0.3f,0.0f,-0.3f,  0.0f,0.0f,0.0f,  0.7071f,-0.7071f,0.0f,0.0f,  0.0f,0.0f,0.0f};
#else
	state_vector attTarget = {0.3f,0.0f,-0.3f,  0.0f,0.0f,0.0f,  0.7071f,0.7071f,0.0f,0.0f,  0.0f,0.0f,0.0f};
#endif	
	state_vector error;
	extern float KDattitudePD;
	extern float KPattitudePD;
	
	sixDOFSimSetThrusterTimes(firing_times); //Update simulation using specified firing times
	
	//Reset firing times
	memset(firing_times,0,sizeof(prop_time));
	
	padsStateGet(flatFloorState); //Get estimated state directly from estimator
	remapStates(flatFloorState); //Transform from ISS frame to ZR coordinates
	memset(&ctrlControl[FORCE_Z],0,sizeof(float)*4); //Zero out Z forces and all torques
	
	//Run controller for attitude only and mix in the flat floor frame
	findStateError(error, flatFloorState, attTarget);
	ctrlAttitudeNLPDwie(0, 0, 0, 0, KPattitudePD, KDattitudePD, error, ctrlControl);
	ctrlMixWLoc(firing_times, ctrlControl, flatFloorState, 10, 39.0f, FORCE_FRAME_INERTIAL);
	
	
#ifndef ZRFFSIMULATIONCHECK
	//Disable +/- z thrusters
	firing_times->on_time[4] = 0;
	firing_times->on_time[5] = 0;
	firing_times->on_time[10] = 0;
	firing_times->on_time[11] = 0;
	firing_times->off_time[4] = 0;
	firing_times->off_time[5] = 0;
	firing_times->off_time[10] = 0;
	firing_times->off_time[11] = 0;
#endif
	
}
#endif //end ifdef ZRFLATFLOOR //
